/**
 * @brief    
 * 
 * @version  0.1
 * @author   wjw
 * @date     2025.01.16
 *         _
 *     ___| |__   ___ _ __   __ _  ___  _ __   __ _
 *    / __| '_ \ / _ \ '_ \ / _` |/ _ \| '_ \ / _` |
 *    \__ \ | | |  __/ | | | (_| | (_) | | | | (_| |
 *    |___/_| |_|\___|_| |_|\__, |\___/|_| |_|\__, |
 *                          |___/             |___/
 * @copyright (c) 2025 by 神工仿真, All Rights Reserved.
 */
#include "MTRAModel.h"

#include "Utility/Algebra/sgPardiso.h"
#include "Utility/Logging/SGLogger.h"

using namespace Kernel;

//=============================================================================
// MTRAModel

MTRAModel::MTRAModel (Kernel::MModel myModel, const SG::Algebra::Mat_t& phi, const SG::Algebra::Vec_t& eigen)
    : m_myModel (myModel), m_phi (phi), m_eigen (eigen), m_jacobian_computed (false)
{}

MTRAModel::~MTRAModel ()
{
    // 清理缓存的矩阵
    m_cached_dfdu.resize (0, 0);
    m_cached_dfdv.resize (0, 0);
    m_cached_dfda.resize (0, 0);
}

void MTRAModel::ComputeDerivatives (const SG::Algebra::Real_t& t,
                                    const SG::Algebra::Vec_t&  u,
                                    const SG::Algebra::Vec_t&  v,
                                    SG::Algebra::Vec_t&        a)
{
    SG::Algebra::SpMat_t K, C, M;
    this->ComputeImplicitJacobian (t, u, v, a, K, C, M);

    SG::Algebra::Vec_t F;
    this->ComputeExplicitValue (t, u, v, F);

    SG::Algebra::Vec_t R = F - (K * u + C * v);
    SG::Algebra::TPardiso<SG::Algebra::Real_t> solver;
    solver.compute (M);
    solver.solve (R, a);
}

void MTRAModel::ComputeImplicitValue (const SG::Algebra::Real_t& t,
                                      const SG::Algebra::Vec_t&  u,
                                      const SG::Algebra::Vec_t&  v,
                                      const SG::Algebra::Vec_t&  a,
                                      SG::Algebra::Vec_t&        f)
{
    SGWarn << __FILE__ << ":" << __LINE__ << " NOT IMPLEMENTED";
}

void MTRAModel::ComputeExplicitValue (const SG::Algebra::Real_t& t,
                                      const SG::Algebra::Vec_t&  u,
                                      const SG::Algebra::Vec_t&  v,
                                      SG::Algebra::Vec_t& _OUT   g)
{
    SG::Algebra::Vec_t F_kp1;
    m_myModel.ComputeExplicitValue (t, u, v, F_kp1);
    g = m_phi.transpose () * F_kp1;
}

void MTRAModel::ComputeImplicitJacobian (const SG::Algebra::Real_t& t,
                                         const SG::Algebra::Vec_t&  u,
                                         const SG::Algebra::Vec_t&  v,
                                         const SG::Algebra::Vec_t&  a,
                                         SG::Algebra::SpMat_t&      dfdu,
                                         SG::Algebra::SpMat_t&      dfdv,
                                         SG::Algebra::SpMat_t&      dfda)
{
    // 如果已经计算过，直接返回缓存的结果
    if (m_jacobian_computed)
    {
        dfdu = m_cached_dfdu;
        dfdv = m_cached_dfdv;
        dfda = m_cached_dfda;
        return;
    }

    SG::Algebra::SpMat_t M, K, C;
    m_myModel.ComputeStiffnessMatrix (_OUT K);
    m_myModel.ComputeDampingMatrix (_OUT C);
    m_myModel.ComputeMassMatrix (_OUT M);

    // 计算模态矩阵
    Eigen::MatrixXd modalK = m_phi.transpose () * K * m_phi;
    Eigen::MatrixXd modalC = m_phi.transpose () * C * m_phi;
    Eigen::MatrixXd modalM = m_phi.transpose () * M * m_phi;

    // 添加模态阻尼
    SG::Algebra::Real_t xi;
    for (int i = 0; i < m_eigen.size(); ++i)
    {
        const auto omega = sqrt (m_eigen[i]);
        xi = m_myModel.ComputeDampingRatio (omega);
        modalC (i, i) += xi * (2.0 * omega);
    }

    // 存储计算结果到缓存中
    m_cached_dfdu = modalK.sparseView ();
    m_cached_dfdv = modalC.sparseView ();
    m_cached_dfda = modalM.sparseView ();

    // 设置已计算标志
    m_jacobian_computed = true;

    // 返回结果
    dfdu = m_cached_dfdu;
    dfdv = m_cached_dfdv;
    dfda = m_cached_dfda;
}

void MTRAModel::ComputeExplicitJacobian (const SG::Algebra::Real_t& t,
                                         const SG::Algebra::Vec_t&  u,
                                         const SG::Algebra::Vec_t&  v,
                                         SG::Algebra::SpMat_t&      dgdu,
                                         SG::Algebra::SpMat_t&      dgdv)
{
    SGWarn << __FILE__ << ":" << __LINE__ << " NOT IMPLEMENTED";
}
